%author: wxj233
%time: 2023.11.9 10:00
%function:  追踪器

classdef MFPTracer < handle
    %FEATURETRACER 多特征点追踪器
    %   此处显示详细说明
    
    properties
        Ips  % initPoolPoints,起始池用于存放用于起始轨迹的点，[1t, 2x, 3y, 4id(仿真真实目标id); ...]

        Tracks  % initTracks,初始轨迹,[track1, track2, ...]

        lastTime  % 上一个时刻
        nowTime  % 当前时刻

        vMax  % 目标最大速度
        r  % 量测误差方差量级
        q  % 过程噪声量级
        p  % Velocity proportion  自身速度在融合速度中所占的比例
        vAssDoor  % 特征点速度关联概率门限
        inDoor  % 数据关联门限（落入波门内的概率）
        fplifeMax  % feature point life,特征点最大生命值
        tlifeMax  % track life,轨迹当前生命值

        epsilon  % dbscan 聚类半径
        minpts  % dbscan聚类核心数
        zoomX  % x轴缩放参数
        zoomY  % y轴缩放参数

        deadFps  % 用于记录死亡的特征点轨迹
    end
    
    methods
        function obj = MFPTracer(vMax, r, q, p, vAssDoor, inDoor, fplifeMax, tlifeMax)
            % 构造此类的实例
            % 完成初始化工作
            % vMax 最大速度
            % r 量测方差数量级
            % q: 过程噪声数量级
            % vp: 自身速度在融合速度中所占的比例取[0, 1]之间,0是不相关，1是强相关
            % vAssDoor 速度关联波门
            % inDoor: 数据关联门限（落入波门内的概率）
            % fplifeMax 特征点最大生命值
            % tlifeMax 轨迹最大生命值
            obj.Ips = [];
            obj.Tracks = struct([]);
            obj.deadFps = struct([]);
            obj.lastTime = 0;
            obj.nowTime = 0;

            obj.vMax = vMax;
            obj.r = r;
            obj.q = q;
            obj.p = p;
            obj.vAssDoor = vAssDoor;
            obj.inDoor = inDoor;
            obj.fplifeMax = fplifeMax;
            obj.tlifeMax = tlifeMax;

            Track.getId(0);  % 初始化轨迹ID编号为0
            FeaturePoint.geId(0);  % 初始化特征点ID编号为0
        end
        

        function points = preproccess(obj, points, epsilon, minpts, zoomX, zoomY)
            % 进行数据预处理，聚类，噪声滤除等
            % points 待处理量测
            % epsilon 聚类半径
            % minpts 聚类核最小量测数
            % zoomX: x轴缩放
            % zoomY: y轴缩放
            % return points: 预处理之后的点[t, x, y, id, cluster; ...]
            if isempty(points)
                return;
            end

            obj.epsilon = epsilon;
            obj.minpts = minpts;
            obj.zoomX = zoomX;
            obj.zoomY = zoomY;
            cluster = dbscan([zoomX*points(:,2), zoomY*points(:,3)], epsilon, minpts,"Distance","euclidean");
            points = [points, cluster];
        end


        function tracer(obj, points)
            % 追踪入口，在此处完成轨迹关联、
            % 此处显示详细说明
            % points 待处理的点[t, x, y, id, cluster; ...]
            if isempty(points)
                return;
            end
            obj.lastTime = obj.nowTime;
            obj.nowTime = points(1,1);

            offPs = obj.trackAss(points);  % 初始轨迹关联
            offPs = obj.trackExtend(offPs);  % 轨迹拓展
            offPs = obj.trackInit(offPs);  % 轨迹起始
            obj.Ips = offPs;  % 最终剩余的游离点放入起始池
        end


        function offPs = trackAss(obj, points)
            % 完成轨迹关联
            % points: 待关联的点[t, x, y, id, cluster; ...]
            % return offPs: 返回没有关联成功的离群点
            if isempty(obj.Tracks)
                offPs = points;
                return;
            end
            dt = obj.nowTime - obj.lastTime;
            fps = struct([]);  % 用于保存所有track中的fps的引用
            costM = [];  % 代价矩阵，用于保存特征点到每一个量测之间的代价(距离)
            for t = 1:1:length(obj.Tracks)
                obj.Tracks(t).vfusion(obj.p);
                obj.Tracks(t).predict(dt, obj.q);  % 先完成预测才可以求得信息这些东西
                for i = 1:1:length(obj.Tracks(t).fps)
                    fp = obj.Tracks(t).fps(i);  % 获取到单个特征点
                    costs = fp.costs(points, obj.inDoor, obj.r);  % 代价计算
                    costM = [costM; costs];
                    fps = [fps, fp];  % 建立对当前特征点的索引，方便后续读取
                end
                obj.Tracks(t).validFpNum = 0;  % 新一轮的关联开始了，原状态置0
            end

            [ass, costM] = obj.hungarian(-costM);  % 匹配最优,转换为最小代价
            useP = [];
            for i = 1:1:length(fps)
                if costM(i, ass(i)) < 0  % 有可关联点
                    fps(i).assP = points(ass(i), :);
                    useP = [useP, ass(i)];
                    fps(i).track.validFpNum = fps(i).track.validFpNum + 1;
                else
                    fps(i).assP = [];
                end
            end

            % 更新
            for track = obj.Tracks
                track.update(obj.r, obj.nowTime);
            end

            points(useP, :) = [];  % 去除已关联的点
            offPs = points;
        end


        function offPs = trackExtend(obj, points)
            % 轨迹特征点拓展
            % points: 用于关联的点,[t, x, y, id, cluster; ...]
            if isempty(obj.Tracks)
                offPs = points;
                return;
            end
            dts = obj.nowTime - obj.Ips(:, 1);
            vxAssM = (points(:, 2)' - obj.Ips(:, 2))./dts;  % velocity Associate Matrix of x
            vyAssM = (points(:, 3)' - obj.Ips(:, 3))./dts;  % velocity Associate Matrix of y
            vAssM = cat(3, vxAssM, vyAssM); % velocity Associate Matrix
            vScalarAssM = sqrt(vxAssM.*vxAssM + vyAssM.*vyAssM);  % 标量速度关联矩阵
            I = find(abs(vScalarAssM) < obj.vMax);  % 用速度筛一下是为了减轻后续的计算量
            [rows, cols] = ind2sub(size(vScalarAssM), I);  % 获取速度小于阈值的点的索引,rows对应obj.Ips的索引，cols对应points的索引
            covars = [];
            for dt = dts'
                covar = [2*obj.r*obj.r, 0;  % 误差协方差矩阵
                         0, 2*obj.r*obj.r].*(1/(dt*dt)*eye(2));
                covars = cat(3, covars, covar);
            end

            dI = [];  % 记录需要被删除的点
            for track = obj.Tracks
                gps = reshape(track.gXk_e, 4, [])';  % 群状态中的所有点
                for i = 1:1:length(rows)  % 遍历所哟速度组合
                    ps = [gps(:,[2,4]); points(cols(i), [2,3])];  % 将群状态和要判断的点拼接起来，聚类进行判断
                    cluster = dbscan([obj.zoomX*ps(:,1), obj.zoomY*ps(:,2)], obj.epsilon, obj.minpts,"Distance","euclidean");
                    if cluster(end) ~= -1  % 说明是邻近点
                        vdiff = track.points(end, [3,5])' - squeeze(vAssM(rows(i), cols(i), :));
                        var = track.mPcov([2,4], [2,4]) + covars(rows(i));
                        D = vdiff'/var*vdiff;
                        if D < chi2inv(0.95, 2)
                            % 说明是可以拓展的特征点
                            track.addFp(obj.Ips(rows(i), :), points(cols(i), :), obj.r, obj.fplifeMax);
                            dI = [dI, cols(i)];
                        end
                    end
                end
            end

            dI = unique(dI);
            points(dI, :) = [];
            offPs = points;
        end


        function offPs = trackInit(obj, points)
            % 轨迹初始化
            % points: 用于轨迹初始化的点[t, x, y, id, cluster; ...]
            if ~isempty(obj.Ips)
                % 轨迹初始化，并将达到成熟轨迹条件的初始轨迹转为成熟轨迹
                dt = obj.nowTime - obj.lastTime;

                vxAssM = (points(:, 2)' - obj.Ips(:, 2))/dt;  % velocity Associate Matrix of x
                vyAssM = (points(:, 3)' - obj.Ips(:, 3))/dt;  % velocity Associate Matrix of y
                vAssM = cat(3, vxAssM, vyAssM); % velocity Associate Matrix
                vScalarAssM = sqrt(vxAssM.*vxAssM + vyAssM.*vyAssM);  % 标量速度关联矩阵
                I = find(abs(vScalarAssM) < obj.vMax);
                [rows, cols] = ind2sub(size(vScalarAssM), I);  % 获取速度小于阈值的点的索引,rows对应obj.Ips的索引，cols对应points的索引
                covar = [2*obj.r*obj.r/(dt*dt), 0;  % 误差协方差矩阵
                         0, 2*obj.r*obj.r/(dt*dt)];

                assedv = [];  % associated velocity记录已经完成关联了的速度的索引
                for i = 1:1:length(rows)
                    if ismember(i, assedv)
                        continue;
                    end
                    track = Track(obj.tlifeMax, obj);  % 创建一个新的轨迹
                    for j = 1:1:length(rows)
                        if ismember(j, assedv)
                            continue;
                        end
                        isSameCluster = obj.Ips(rows(i), 5) == obj.Ips(rows(j), 5);  % 这个地方可能有问题
                        v_diff = squeeze(vAssM(rows(i), cols(i), :) - vAssM(rows(j), cols(j), :));  % squeeze用于消除单一维
                        D = v_diff'/covar*v_diff;
                        if isSameCluster && D < chi2inv(obj.vAssDoor, 2)  % 这个地方可以思考下，如果刚好是哪个5%每匹配上怎么办，会出现多个轨迹
                            track.addFp(obj.Ips(rows(j), :), points(cols(j), :), obj.r, obj.fplifeMax);
                            assedv = [assedv, j];
                        end
                    end
                    obj.Tracks = [obj.Tracks, track];
                end
                points(cols, :) = [];  % 删除被用了的点
            end
            offPs = points;  % 返回起始后剩余的游离点
        end


        function [ass, costM] = hungarian(~, costM)
            % 匈牙利算法实现最佳匹配
            % costM: 代价矩阵
            % return 匹配情况[t, j; ...]第t个特征点轨迹匹配第j个点
            [fpt, pNum] = size(costM);  % 获取特征点轨迹数量和待关联点数量
            if fpt > pNum  % 补成方阵
                num = fpt - pNum;
                patch = zeros([fpt, num]);  % 0代表不可能
                costM = [costM, patch];
            else
                num = pNum - fpt;
                patch = zeros([num, pNum]);  % 0代替不可能
                costM = [costM; patch];
            end
            [ass, ~] = munkres(costM);
        end


        function deleteTrack(obj, id)
            % 删除某一条指定轨迹
            % id: 轨迹id
            for t = 1:1:length(obj.Tracks)
                if obj.Tracks(t).id == id
                    for fp = obj.Tracks(t).fps
                        obj.deadFps = [obj.deadFps, fp];
                    end
                    obj.Tracks(t) = [];
                    return;
                end
            end
        end

    end
end

